package logic.algorithm;

import java.util.Iterator;
import java.util.LinkedList;
import java.util.List;
import java.util.PriorityQueue;
import java.util.Vector;

import datamodel.Grid;
import datamodel.GridElement;
/**
 * 
 */

/**
 * @author toa, Michelle, Steffen
 * @version final
 *
 */
public class AStar implements Algorithm {
	
	boolean solved = false;
	Grid grid = null;
	PriorityQueue<GridElement> openQueue = null;
	List<GridElement> closeList = null;
	
	GridElement start = null;
	GridElement goal  = null;
	
	int tentative_g_score = 0;
	boolean tentative_better = false;
	
	
	/* (non-Javadoc)
	 * @see logic.algorithm.Algorithm#init(datamodel.Grid)
	 */
	@Override
	public void init(Grid grid) {
		this.grid = grid;
		this.openQueue = new PriorityQueue<GridElement>(1,new ComparatorEstimateDistance());
		this.closeList = new LinkedList<GridElement>();
		
		start = this.grid.getStartElement();
		goal  = this.grid.getEndElement();
		
		start.setDistance(0);
		int estimateDistance = heuristic_cost_estimate(start,goal);
		
		start.setEstimateDistance(estimateDistance);
		start.setHeuristic(estimateDistance);
		
		openQueue.offer(start);
	}

	/* (non-Javadoc)
	 * @see logic.algorithm.Algorithm#doNextStep()
	 */
	@Override
	public boolean doNextStep() {
		
		
		if(openQueue.isEmpty()){
			solved = true;
			return false;
		}
		
		//getting the current node from the queue
		GridElement current = openQueue.peek();
		
		//reached the end node?
		if(current == goal){
			solved = true;
			return false;
		}
		
		//delete from openList
		openQueue.remove(current);
		
		//set up a vector of all diagonal neighbors
		Vector<GridElement> neighbors = grid.getNeighborsFrom(current, true).getNeighbors();
		//add the horizontal and vertical neighbors to the vector
		neighbors.addAll(grid.getNeighborsFrom(current, false).getNeighbors());
		
		// get the way cost to all neighbors
		Vector<Integer> wayCosts = grid.getNeighborsFrom(current, true).getWayCosts();
		wayCosts.addAll(grid.getNeighborsFrom(current, false).getWayCosts());
		
		//add an iterator to the way cost vector
		Iterator<Integer> it = wayCosts.iterator();
		
		//as long as there are elements in the vector of neighbors, proceed
		for(GridElement neighbor: neighbors){
			
			//do not care about neighbors already known or max valued
			if(closeList.contains(neighbor) || neighbor.getState().getWayCost() == Integer.MAX_VALUE){
				//discard this iterator and go to the next node
				it.next();
				continue;
			}
			
			//add way cost of the neighbor to the current distance 
			this.tentative_g_score = current.getDistance() + it.next();
			
			//do not move away from the goal
			if(openQueue.contains(neighbor) && this.tentative_g_score >= neighbor.getDistance())
				continue;
			
			
			neighbor.setPrecursor(current);
			neighbor.setDistance(this.tentative_g_score);
			
			//calculate the minimum distance from this neighbor to the goal
			int estimateDistance = this.tentative_g_score + this.heuristic_cost_estimate(neighbor, goal);
			neighbor.setEstimateDistance(estimateDistance);
			
			//insert the neighbor into the openQuene
			if(openQueue.contains(neighbor)){
				//since the estimate distance of the neighbor has changed place it at a new position
				openQueue.remove(neighbor);
				openQueue.offer(neighbor);
			}
			else
				openQueue.offer(neighbor);
			
		}
		//node finished
		closeList.add(current);
		
		return true;
	} 
	
	/**
	 * Calculates heuristic cost from start to end node using the given nodes.
	 * Heuristic is the euclidian norm but distance between these 2 nodes is
	 * multiplied by 4 to avoid weak heuristics.
	 *
	 * @param start given start node
	 * @param goal given end node
	 *
	 * @return calculated heuristic value
	 */
	private int heuristic_cost_estimate(GridElement start, GridElement goal){
		
		
		int distance1 = Math.abs(start.getRow() - goal.getRow());
		int distance2 = Math.abs(start.getColumn() - goal.getColumn());

		distance1 *= 4;
		distance2 *= 4;
		
		return (int) Math.sqrt(Math.pow(distance1, 2) + Math.pow(distance2, 2));
	}

	/* (non-Javadoc)
	 * @see logic.algorithm.Algorithm#isSolved()
	 */
	@Override
	public boolean isSolved() {
		return solved;
	}

}
